#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include <cstdlib>
#include <math.h>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include <fstream>
#include<iomanip>
#include "../include/blue/Person.h"

//LightDescriptor类成员函数

LightDescriptor::LightDescriptor(const cv::RotatedRect& light)
{
    width = light.size.width;
    length = light.size.height;
    center = light.center;
    angle = light.angle;
    area = light.size.area();
    brect = light.boundingRect();
}

const LightDescriptor& LightDescriptor::operator =(const LightDescriptor& ld)
{
    this->width = ld.width;
    this->length = ld.length;
    this->center = ld.center;
    this->angle = ld.angle;
    this->area = ld.area;
    this->brect=ld.brect;
    return *this;
}

cv::RotatedRect LightDescriptor::rec() const
{
    return cv::RotatedRect(center, cv::Size2f(width, length), angle);
}

void  LightDescriptor::draw(cv::Mat img)
{
        cv::rectangle(img, brect, cv::Scalar(0, 255, 0), 2);
}

//MatchLights类函数
MatchLights::MatchLights(const LightDescriptor& light1,const LightDescriptor& light2)
{
    double pi=3.14159265;
    center=(light1.center+light2.center)/2;
    A.x = light1.center.x+sin(light1.angle/180*pi)*light1.length/2;
    A.y=light1.center.y-cos(light1.angle/180*pi)*light1.length/2;
    B.x=light2.center.x+sin(light2.angle/180*pi)*light2.length/2;
    B.y=light2.center.y-cos(light2.angle/180*pi)*light2.length/2;
    C=(light2.center+light2.center)-B;
    D=(light1.center+light1.center)-A;

    //打分项
    qx=(light1.center.y-light2.center.y)/(light1.center.x-light2.center.x);
    px=abs((A.y-B.y)/(A.x-B.x)-qx)+abs((D.y-C.y)/(D.x-C.x)-qx);
    ratio=2*abs(light1.center.x-light2.center.x)/(light1.length+light2.length);
    score=2*abs(qx)+px+ratio;

        //四个点的方位
    if(light1.center.x<light2.center.x && A.y<D.y){situation=1;}
    if(light1.center.x<light2.center.x && A.y>D.y){situation=2;}
    if(light1.center.x>light2.center.x && A.y<D.y){situation=3;}
    if(light1.center.x>light2.center.x && A.y>D.y){situation=4;}
}

const MatchLights& MatchLights::operator =(const MatchLights& ld)
{
    this->center = ld.center;
    this->A = ld.A;
    this->B = ld.B;
    this->C = ld.C;
    this->D = ld.D;
    this->qx=ld.qx;
    this->px=ld.px;
    this->ratio=ld.ratio;
    this->score=ld.score;
    this->situation=ld.situation;
    return *this;
}

void MatchLights::draw(cv::Mat img)
{
    cv::line(img,A,B,cv::Scalar(0,255,0),4);
    cv::line(img,B,C,cv::Scalar(0,255,0),4);
    cv::line(img,C,D,cv::Scalar(0,255,0),4);
    cv::line(img,D,A,cv::Scalar(0,255,0),4);
}

void MatchLights::GetFourPoints(std::vector<cv::Point2f> &fourPoints)
{
        
    fourPoints.push_back(A);
    fourPoints.push_back(B);
    fourPoints.push_back(C);
    fourPoints.push_back(D);
}

Eigen::Vector3f MatchLights::PNP(cv::Mat IntrinsicMatrix,cv::Mat coe)
{
    std::vector<cv::Point3f> orig;
    std::vector<cv::Point2f> xszb;
    cv::Mat rvec,tvec,rotMat;
    orig.push_back(cv::Point3f(0,-67.5,31));
    orig.push_back(cv::Point3f(0,67.5,31));
    orig.push_back(cv::Point3f(0,67.5,-31));
    orig.push_back(cv::Point3f(0,-67.5,-31));
    if(situation==1)
    {
        xszb.push_back(A);
        xszb.push_back(B);
        xszb.push_back(C);
        xszb.push_back(D);
    }
    if(situation==2)
    {
        xszb.push_back(D);
        xszb.push_back(C);
        xszb.push_back(B);
        xszb.push_back(A);
    }
    if(situation==3)
    {
        xszb.push_back(B);
        xszb.push_back(A);
        xszb.push_back(D);
        xszb.push_back(C);
    }
    if(situation==4)
    {
        xszb.push_back(C);
        xszb.push_back(D);
        xszb.push_back(B);
        xszb.push_back(A);
    }
    solvePnP(orig,xszb,IntrinsicMatrix,coe,rvec,tvec);
    Rodrigues(rvec, rotMat);

    Eigen::Matrix3f R_n;
    Eigen::Vector3f T_n;
    cv2eigen(rotMat, R_n);
    cv2eigen(tvec, T_n);
    return -R_n.inverse()*T_n;
}


 
 
 
 

 
 
